#include <WebSocketsServer.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include "PubSubClient.h"
#include "camera_wrap.h"
#include "Ticker.h"
#include "SGP30.h"
#include "SHT40.h"
#include "Wire.h"

#define I2C_SDA 15
#define I2C_SCL 14
SGP30 SGP(I2C_SDA,I2C_SCL,&Wire);
SHT40 sht4 = SHT40();
sensors_event_t humidity, temp;
float Co2,Tvoc;
enum TRACK{
  TRACK_NONE = 0,
  TRACK_FW,
  TRACK_LEFT,
  TRACK_RIGHT,
  TRACK_STOP
};
const char* ssid = "Nice";    
const char* password = "12312312";

const char *mqtt_server = "mqtts.heclouds.com"; //onenet 的 IP地址
const int port = 1883;                    //端口号
#define mqtt_pubid "2KYi18U9v7"    //产品ID
#define mqtt_devid "test" //设备名称
//鉴权信息
#define mqtt_password "version=2018-10-31&res=products%2F2KYi18U9v7%2Fdevices%2Ftest&et=2810313321&method=md5&sign=Vu7n0Tfw210k3sWtCQuzvw%3D%3D" //鉴权信息
//设备下发命令的set主题
#define ONENET_TOPIC_PROP_SET "$sys/" mqtt_pubid "/" mqtt_devid "/thing/property/set"
//设备上传数据的post主题
#define ONENET_TOPIC_PROP_POST "$sys/" mqtt_pubid "/" mqtt_devid "/thing/property/post"
//这是post上传数据使用的模板
#define ONENET_POST_BODY_FORMAT "{\"id\":\"%u\",\"params\":%s}"
int postMsgId = 0; //记录已经post了多少条
WiFiClient espClient;           //创建一个WIFI连接客户端
PubSubClient client(espClient); // 创建一个PubSub客户端, 传入创建的WIFI客户端
Ticker tim1;                    //定时器,用来循环上传数据

int cameraInitState = -1;
uint8_t* jpgBuff = new uint8_t[68123];
size_t   jpgLength = 0;
uint8_t camNo=0;
bool clientConnected = false;

WiFiUDP UDPServer;
IPAddress addrRemote;
unsigned int portRemote;
unsigned int UDPPort = 6868;
const int RECVLENGTH = 16;
byte packetBuffer[RECVLENGTH];

WebSocketsServer webSocket = WebSocketsServer(86);
String html_home;

const int LED_BUILT_IN        = 4;
const uint8_t TRACK_DUTY      = 100;
unsigned long previousMillisServo = 0;
const unsigned long intervalServo = 10;

//向主题发送模拟的温湿度数据
void sendMqqt()
{
  sht4.getEvent(&humidity, &temp);
  delay(5);
  SGP.Read();
  delay(5);
  if (client.connected())
  {
    Co2 = SGP.getCO2();
    Tvoc = SGP.getTVOC();
    //先拼接出json字符串
    char param[100];
    char jsonBuf[178];
    sprintf(param, "{\"temp\":{\"value\":%.2f},\"humi\":{\"value\":%.2f},\"VOC\":{\"value\":%.2f},\"Co2\":{\"value\":%.2f}}", temp.temperature, humidity.relative_humidity,Tvoc,Co2); 
    //我们把要上传的数据写在param里
    postMsgId += 1;
    sprintf(jsonBuf, ONENET_POST_BODY_FORMAT, postMsgId, param);
    //再从mqtt客户端中发布post消息
    if (client.publish(ONENET_TOPIC_PROP_POST, jsonBuf))
    {
      // Serial.print("Post message to cloud: ");
      // Serial.println(jsonBuf);
    }
    else
    {
      //Serial.println("Publish message to cloud failed!");
    }
  }
}
 
//重连函数, 如果客户端断线,可以通过此函数重连
void clientReconnect()
{
  while (!client.connected()) //再重连客户端
  {
    //Serial.println("reconnect MQTT...");
    if (client.connect(mqtt_devid, mqtt_pubid, mqtt_password))
    {
      //Serial.println("connected");
    }
    else
    {
      //Serial.println("failed");
      //Serial.println(client.state());
      //Serial.println("try again in 5 sec");
      delay(5000);
    }
  }
}
void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {

  switch(type) {
      case WStype_DISCONNECTED:
          Serial.printf("[%u] Disconnected!\n", num);
          camNo = num;
          clientConnected = false;
          break;
      case WStype_CONNECTED:
          Serial.printf("[%u] Connected!\n", num);
          clientConnected = true;
          break;
      case WStype_TEXT:
      case WStype_BIN:
      case WStype_ERROR:
      case WStype_PING:
      case WStype_PONG:
      case WStype_FRAGMENT_TEXT_START:
      case WStype_FRAGMENT_BIN_START:
      case WStype_FRAGMENT:
      case WStype_FRAGMENT_FIN:
          Serial.println(type);
          break;
  }
}

void processUDPData(){
  int cb = UDPServer.parsePacket();

  if (cb) {
      UDPServer.read(packetBuffer, RECVLENGTH);
      addrRemote = UDPServer.remoteIP();
      portRemote = UDPServer.remotePort();

      String strPackage = String((const char*)packetBuffer);
  #ifdef DEBUG
      Serial.print("receive: ");
      Serial.print(strPackage);
      Serial.print(" from: ");
      Serial.print(addrRemote);
      Serial.print(":");
      Serial.println(portRemote);
  #endif
      if(strPackage.equals("whoami")){
          UDPServer.beginPacket(addrRemote, portRemote-1);
          String res = "ESP32-CAM";
          UDPServer.write((const uint8_t*)res.c_str(),res.length());
          UDPServer.endPacket();
          Serial.println("response");
      }else if(strPackage.equals("forward")){
        Serial.println("go");
      }else if(strPackage.equals("backward")){
        Serial.println("back"); 
      }else if(strPackage.equals("left")){
        Serial.println("left");
      }else if(strPackage.equals("right")){
         Serial.println("right");
      }else if(strPackage.equals("stop")){
         Serial.println("stop");
         //laneoff
      }else if(strPackage.equals("1")){
         Serial.println("1");
      }else if(strPackage.equals("2")){
         Serial.println("2");
      }else if(strPackage.equals("3")){
         Serial.println("3");
      }else if(strPackage.equals("4")){
         Serial.println("4");
      }else if(strPackage.equals("5")){
         Serial.println("5");
      }else if(strPackage.equals("spadd")){
         Serial.println("spadd");
      }else if(strPackage.equals("spsub")){
         Serial.println("spsub");
      }else if(strPackage.equals("ledon")){
        digitalWrite(LED_BUILT_IN, HIGH);
      }else if(strPackage.equals("ledoff")){
        digitalWrite(LED_BUILT_IN, LOW);
      }else if(strPackage.equals("laneon")){
         Serial.println("xunji");
      }else if(strPackage.equals("objecton")){
          Serial.println("objecton");
      }
        
      memset(packetBuffer, 0, RECVLENGTH);
  }

}
void setup() {
  Serial.begin(115200);
  pinMode(LED_BUILT_IN, OUTPUT);
  digitalWrite(LED_BUILT_IN, LOW);
  cameraInitState = initCamera();
  if(cameraInitState != 0){
    return;
  }

  //WIFI INIT
  Serial.printf("Connecting to %s\n", ssid);
  if (String(WiFi.SSID()) != String(ssid)) {
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
  }

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.print("Connected! IP address: ");
  String ipAddress = WiFi.localIP().toString();;
  Serial.println(ipAddress);

  webSocket.begin();
  webSocket.onEvent(webSocketEvent);

  UDPServer.begin(UDPPort); 

  client.setServer(mqtt_server, port);                   //设置客户端连接的服务器,连接Onenet服务器, 使用6002端口
  client.connect(mqtt_devid, mqtt_pubid, mqtt_password); //客户端连接到指定的产品的指定设备.同时输入鉴权信息
  if (client.connected())
  {
    //Serial.println("OneNet is connected!"); //判断以下是不是连好了.
  }
  tim1.attach(10, sendMqqt); //定时每10秒调用一次发送数据函数

  SGP.begin();
  SGP.GenericReset();
  delay(5);
  sht4.begin(I2C_SDA,I2C_SCL,&Wire);
  sht4.setPrecision(SHT40_HIGH_PRECISION);
  sht4.setHeater(SHT40_NO_HEATER);
}

void loop() {
  webSocket.loop();
  if(clientConnected == true){
    grabImage(jpgLength, jpgBuff);
    webSocket.sendBIN(camNo, jpgBuff, jpgLength);
  }  
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillisServo >= intervalServo) {
    previousMillisServo = currentMillis;
    processUDPData();
  }

  if (!client.connected()) //如果客户端没连接ONENET, 重新连接
  {
    clientReconnect();
    delay(100);
  }
  client.loop(); //客户端循环检测

}
